#pragma once
#include "airbag.h"

int airbag[] = {0, 0, 0}; // 当前气囊状态
int lastAirbagStatus[] = {0, 0, 0}; // 上次气囊状态
int press[] = {0, 0, 0}; // 压感输入
int lastpress[] = {0, 0, 0}; //上次压感输入
long time_integral[] = {0, 0, 0}; // 各气囊已充气时长
// unsigned long time_integral = 0;
const long TIME_DELAY = 30000;// 延迟时间, 30s
long time_remaindelay[] = {0, 0, 0}; // 各气囊剩余延迟放气时长

unsigned long lastMillis;
unsigned long SWITCH_TIME = 30000;//30s
bool isAerating = false; //是否正在充气

// 保存实时压感状态输入
int pressStatus[] = {0, 0, 0};

bool isSnoring = false; // 当前鼾声识别结果
bool lastSnoring = false;// 上次鼾声识别结果
bool isPressDiff = true;//yaganweizhishifouyoubianhua

void initAirBag(){
  pinMode(AIRBAG_1, OUTPUT);
  pinMode(AIRBAG_2, OUTPUT);
  pinMode(AIRBAG_3, OUTPUT);
  pinMode(AIR_OUTPUT, OUTPUT);

  pinMode(PILLOW_LEFT, INPUT);
  pinMode(PILLOW_MIDDLE, INPUT);
  pinMode(PILLOW_RIGHT, INPUT);
  pinMode(SNORE_INPUT, INPUT);

  lastMillis = millis();
}

void printMessage(){
  Serial.print("leftInput == HIGH: ");
  Serial.print(pressStatus[0] == HIGH);
  Serial.print("  middleInput == HIGH: ");
  Serial.print(pressStatus[1] == HIGH);
  Serial.print("  rightInput == HIGH: ");
  Serial.print(pressStatus[2] == HIGH);
  Serial.print("  isSnoring == true: ");
  Serial.println(isSnoring == true);

  Serial.print("leftairbag: ");
  Serial.print(airbag[0]);
  Serial.print("  middleairbag: ");
  Serial.print(airbag[1]);
  Serial.print("  rightairbag: ");
  Serial.print(airbag[2]);
  Serial.print("  isAerating: ");
  Serial.println(isAerating);

  Serial.print("left_time_integral: ");
  Serial.print(time_integral[0]);
  Serial.print("  middle_time_integral: ");
  Serial.print(time_integral[1]);
  Serial.print("  right_time_integral: ");
  Serial.println(time_integral[2]);
}

void pressInput(){
  pressStatus[0] = digitalRead(PILLOW_LEFT);
  pressStatus[1] = digitalRead(PILLOW_MIDDLE);
  pressStatus[2] = digitalRead(PILLOW_RIGHT);
}

int snoreInput(){
  lastSnoring = isSnoring;
  isSnoring = digitalRead(SNORE_INPUT);
  return isSnoring;
}

void snoreInput(bool snore_input){
  lastSnoring = isSnoring;
  isSnoring = snore_input;
}

int inputControl(int input){
  if(input == '1'){
    clearPressInput();
    pressStatus[0] = HIGH;
    isSnoring = true;

  }
  if(input == '2'){
    clearPressInput();
    pressStatus[1] = HIGH;
    isSnoring = true;

  }
  if(input == '3'){
    clearPressInput();
    pressStatus[2] = HIGH;
    isSnoring = true;

  }
  if(input == '4'){
    clearPressInput();
    isSnoring = false;
  }
  return isSnoring;
}

void airbagControl(){
  airbagControl(pressStatus, isSnoring);
}

void airbagControl(const int PID[3], bool predict){

  lastpress[0] = press[0];
  lastpress[1] = press[1];
  lastpress[2] = press[2];
  press[0] = PID[0];
  press[1] = PID[1];
  press[2] = PID[2];

  if(!isPress(press)){
    //无压感
    setAirBag(LOW, LOW, LOW, false);
  }else if(isPress(press) && !predict){
    //有压感无鼾声
    if(lastSnoring && isPress(lastpress)){
      //延迟放气，干预成功
      for(int i = 0; i < POSITION_NUMBER; i++){
        time_remaindelay[i] = TIME_DELAY;
      }
      setAirBag(airbag[0], airbag[1], airbag[2], false);
    }else{
      setAirBag(LOW, LOW, LOW, false);
    }
  }else{
    //有压感有鼾声
    if(!(lastSnoring && isPress(lastpress))){
      if(!lastSnoring && isPress(lastpress) && isAirbagFill() && isPressSame()){
        setAirBag(airbag[0], airbag[1], airbag[2], true);
        isPressDiff = false;;
      }else{
        if(press[0] == HIGH){
          setAirBag(LOW, HIGH, LOW, true);
        }else if(press[1] == HIGH){
          setAirBag(HIGH, LOW, LOW, true);
        }else if(press[2] == HIGH){
          setAirBag(LOW, HIGH, LOW, true);
        }
        if(isPressSame()){
          isPressDiff = false;
        }else{
          isPressDiff = true;  
        }
      }
    }else{
      if(isPressSame()){
        if(!isPressDiff){
          //干预失败，继续干预
          setAirBag(airbag[0], airbag[1], airbag[2], true);
        }else{
          if(press[0] == HIGH){
            setAirBag(LOW, HIGH, LOW, true);
          }else if(press[1] == HIGH){
            setAirBag(HIGH, LOW, LOW, true);
          }else if(press[2] == HIGH){
            setAirBag(LOW, HIGH, LOW, true);
          }
        }
        isPressDiff = false;
      }else{
        setAirBag(airbag[0], airbag[1], airbag[2], false);
        isPressDiff = true;
      }
    }
  }

}

void updateAirBagStatus(){
    // 获取数组的长度
// int length = sizeof(airbag) / sizeof(airbag[0]);
  for (int i = 0; i < POSITION_NUMBER; i++) {
    if(airbag[i] == LOW){
        time_integral[i] -= (millis() - lastMillis);
        if(time_integral[i] < 0){
          time_integral[i] = 0;
        }
    }else if(airbag[i] == HIGH){
        time_integral[i] += (millis() - lastMillis);
        if(time_integral[i] >= SWITCH_TIME*2){
          time_integral[i] = SWITCH_TIME*2;
        }
        if(time_integral[i] >= SWITCH_TIME){
            aerate(false);
            if(time_integral[i] >= SWITCH_TIME*2){
              if(i-2 >= 0 && press[i-1]){
                setAirBag(HIGH, LOW, LOW, true);
                time_integral[i] = SWITCH_TIME*2 - 1; //
                time_integral[i-2] = 0;
              }else if(i+2 < 3 && press[i+1]){
                setAirBag(LOW, LOW, HIGH, true);
                time_integral[i] = SWITCH_TIME*2 - 1;
                time_integral[i+2] = 0;
              }
            }
        }
    }
  }
  lastMillis = millis();
}

void setAirBag(int left, int middle, int right, bool isAerate){
  airbag[0] = left;
  airbag[1] = middle;
  airbag[2] = right;

  digitalWrite(AIRBAG_1, airbag[0]);
  digitalWrite(AIRBAG_2, airbag[1]);
  digitalWrite(AIRBAG_3, airbag[2]);
  aerate(isAerate);
  
  lastAirbagStatus[0] = airbag[0];
  lastAirbagStatus[1] = airbag[1];
  lastAirbagStatus[2] = airbag[2];

}

void clearPressInput(){
  pressStatus[0] = LOW;
  pressStatus[1] = LOW;
  pressStatus[2] = LOW;
}

int isPress(const int PID[3]){
  return PID[0] || PID[1] || PID[2];
}

int isPressSame(){
  return (lastpress[0] == press[0]) && (lastpress[1] == press[1]) && (lastpress[2] == press[2]);
}

int isAirbagFill(){
  return airbag[0] || airbag[1] || airbag[2];
}

// int isDelay(){
//   for(int i = 0; i < POSITION_NUMBER; i++){
//         time_remaindelay[i] = TIME_DELAY;
//       }
// }

void aerate(bool isAerate){
  if(isAerate){
    digitalWrite(AIR_OUTPUT, HIGH);
    isAerating = true;
  }else{
    digitalWrite(AIR_OUTPUT, LOW);
    isAerating = false;
  }
}
